#include <ros/ros.h>
#include <iostream>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>

using namespace std;
using namespace cv;
// #define uint32_t shape = visualization_msgs::Marker::CUBE

#define red 0
#define blue 1

void drawRect(Mat src,RotatedRect temp)
{
    // 绘制旋转矩阵
    Point2f temp_[4];
    temp.points(temp_);
    for(int i = 0; i < 4; i++)
    {
        line(src, temp_[i], temp_[(i+1) % 4], Scalar(255,12,12), 2, 8);
    }
    return;
}

Point getRotateRectCenter(RotatedRect temp1)
{
    Point2f edge_points[4];
    temp1.points(edge_points);
    return Point((edge_points[0].x + edge_points[2].x)/2,(edge_points[0].y + edge_points[2].y)/2);
}

string convInttosring(int a)
{
    char s[40];
    string s_;
    sprintf(s,"%d",a);
    s_ = s;
    return s_;
}

Point3d Calculate(Point3f point3D,Mat RT)
{    
    /* 
    RT为旋转矩阵和平移矩阵合起来的矩阵。
    RT = [R|T]
    */
    Mat xyz1(4,1,CV_64F,Scalar::all(0));
    xyz1.at<double>(0,0) = point3D.x;
    xyz1.at<double>(1,0) = point3D.y;
    xyz1.at<double>(2,0) = point3D.z;
    xyz1.at<double>(3,0) = 1.;

    Mat temp = RT*xyz1;
    return Point3d(temp.at<double>(0,0),temp.at<double>(1,0),temp.at<double>(2,0));
}

int main(int argc, char** argv)
{
    // 初始化一个节点，创建话题
    uint32_t shape = visualization_msgs::Marker::CUBE;
    ros::init(argc, argv, "show");
    ros::NodeHandle nh1,nh2;
    ros::Publisher pub1 = nh1.advertise<sensor_msgs::Image>("image", 1);
    ros::Publisher pub2 = nh2.advertise<visualization_msgs::Marker>("Marker",1);
    
    sensor_msgs::ImagePtr msg;
    visualization_msgs::Marker marker;
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    marker.ns = "basic_shapes";
    marker.id = 1;
    marker.type = shape;

    ros::Rate looprate(24);          //一秒24帧恰好达到播放视频流的效果

    int color_flag = red;
    Mat src;
    Mat midImage;
    Mat binImage;

    // 相机内参
    Mat cameraMatrix;
    Mat distCoeffs;

    // 计算相机外参数
    double s;
    Mat rotateMatrix ;
    Mat tvec         ;
    Mat rvec         ;

    RotatedRect rotate_Rect;
    Rect rect;
    vector<Mat> channels;
    vector<vector<Point>> contours;
    vector<vector<Point>> contours_;
    vector<Vec4i> hierarchy;
    vector<RotatedRect> rotate_Rect_Group;
    vector<Rect> rect_Group;
    VideoCapture cap;
    vector<Point> interstPoints; //匹配的燈條對應的裝甲板的角點

    // pnp求解所需的点集
    vector<Point3f> markPoint3Ds;
    vector<Point2f> markPoint2Ds;
    Point left_up,left_down,right_up,right_down;

    // 摄像机坐标系下四个角点坐标
    Point3d left_up3D,left_down3D,right_up3D,right_down3D;

    // 旋转矩阵和平移矩阵的组合矩阵[R|T]
    Mat RT_;

while(ros::ok()){
    cap.open("/home/czs/桌面/water.avi");
while(1){
    if(!cap.read(src)) break;
    cap.read(src);
    Mat black(src.size(),CV_8UC1,Scalar(0,0,0));        //用于检测角点
    GaussianBlur(src, midImage, Size(3,3), 0);        
    split(midImage, channels);
    if(color_flag == red)
    {
        midImage = channels.at(2) - channels.at(0);
    }else
    {
        midImage = channels.at(0) - channels.at(2);
    }
    double th = threshold(midImage, binImage, 40, 255, THRESH_BINARY|THRESH_OTSU);   
    if(th-10 > 0)
    {
        threshold(midImage,binImage, th-10, 255, THRESH_BINARY);
    }
    findContours(binImage,contours,hierarchy,RETR_CCOMP,CHAIN_APPROX_NONE);

    for(int i = 0; i < contours.size(); i++)
    {
        float contour_area = contourArea(contours[i]);
        if(contour_area < 100 || contour_area > 3000) continue;
        rotate_Rect = minAreaRect(contours[i]);
        rect = boundingRect(Mat(contours[i]));               //拟合所有轮廓的直立矩形
        rotate_Rect_Group.push_back(rotate_Rect);
        rect_Group.push_back(rect);
        contours_.push_back(contours[i]);
        // drawRect(src,rotate_Rect);
        // debug 用
        // cout<<convInttosring(i)<<":"<<rotate_Rect.size.width<<" "<<rotate_Rect.size.height<<" "<<rotate_Rect.angle<<endl;
    }
    // 似乎这个视频中不用配对，因为只有两条
    Point center1;
    Point center2;

    for(int i = 0; i < contours_.size(); i++)
    {
        for(int j = 0; j < contours_.size(); j++)
        {
            center1 = getRotateRectCenter(rotate_Rect_Group[i]);
            center2 = getRotateRectCenter(rotate_Rect_Group[j]);
            if(center1.x < center2.x)
            {
                if(rotate_Rect_Group[i].size.width > rotate_Rect_Group[i].size.height)
                {
                    left_up.x   = center1.x + cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.width/2;
                    left_down.x = center1.x - cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.width/2;
                    left_up.y   = center1.y - abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.width/2;
                    left_down.y = center1.y + abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.width/2;
                }
                else
                {
                    left_up.y   = center1.y - cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.height/2;
                    left_down.y = center1.y + cos(rotate_Rect_Group[i].angle/180 * CV_PI)*rotate_Rect_Group[i].size.height/2;
                    left_up.x   = center1.x - abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.height/2;
                    left_down.x = center1.x + abs(sin(rotate_Rect_Group[i].angle/180 * CV_PI))*rotate_Rect_Group[i].size.height/2;
                    
                }
                if(rotate_Rect_Group[j].size.width > rotate_Rect_Group[j].size.height)
                {
                    right_up.x  = center2.x + cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.width/2;
                    right_down.x= center2.x - cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.width/2;
                    right_up.y  = center2.y - abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.width/2;
                    right_down.y= center2.y + abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.width/2;
                }
                else
                {
                    right_up.y  = center2.y - cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.height/2;
                    right_down.y= center2.y + cos(rotate_Rect_Group[j].angle/180 * CV_PI)*rotate_Rect_Group[j].size.height/2;
                    right_up.x  = center2.x - abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.height/2;
                    right_down.x= center2.x + abs(sin(rotate_Rect_Group[j].angle/180 * CV_PI))*rotate_Rect_Group[j].size.height/2;
                }
                break;
            }
            else
            {

            }
        }
    }

    // if(rotate_Rect_Group.size() < 2) continue;
    // 检查角点的方法不稳定，所以弃用了
    // interstPoints = detectpoints(black, rotate_Rect_Group[0], rotate_Rect_Group[1]);
    // rotate_Rect_Group.clear();
    // left_up    = interstPoints[0];
    // left_down  = interstPoints[3];
    // right_up   = interstPoints[1];
    // right_down = interstPoints[2];
    // interstPoints.clear();      //vector容器得及时清理以免影响下次循环
    circle(src, right_down, 5, Scalar(255,255,255));
    circle(src, left_down, 5, Scalar(255,255,255));
    circle(src, right_up, 5, Scalar(255,255,255));
    circle(src, left_up, 5, Scalar(255,255,255));
    line(src, left_down, right_up, Scalar(255,255,255), 2);
    line(src, left_up, right_down, Scalar(255,255,255), 2);
    // cout<<black<<endl;
    // imshow("black", src);
    // waitKey(0);

    // 相机内参，畸变系数定义：
    cameraMatrix = Mat(3,3,CV_64F,Scalar::all(0));
    distCoeffs   = Mat(1,5,CV_64F,Scalar::all(0));
    double zConst    = 0;

    // 赋值:

    cameraMatrix.at<double>(0,0) = 1128.048344;
    cameraMatrix.at<double>(0,2) = 339.421769;
    cameraMatrix.at<double>(1,1) = 1127.052190;
    cameraMatrix.at<double>(1,2) = 236.535242;
    cameraMatrix.at<double>(2,2) = 1.;

    distCoeffs.at<double>(0,0) = -0.568429;
    distCoeffs.at<double>(0,1) = 0.514592;
    distCoeffs.at<double>(0,2) = -0.000126;
    distCoeffs.at<double>(0,3) = 0.000500;
    distCoeffs.at<double>(0,4) = 0.000000;

    markPoint3Ds.push_back(cv::Point3f(-67.5f,-27.5f,0));
    markPoint3Ds.push_back(cv::Point3f( 67.5f,-27.5f,0));
    markPoint3Ds.push_back(cv::Point3f( 67.5f, 27.5f,0));
    markPoint3Ds.push_back(cv::Point3f(-67.5f, 27.5f,0));

    markPoint2Ds.push_back(left_up);
    markPoint2Ds.push_back(right_up);
    markPoint2Ds.push_back(right_down);
    markPoint2Ds.push_back(left_down);

    // 得到旋转矩阵和平移矩阵，实际上平移矩阵就是世界坐标系原点即装甲板中点在摄像机坐标系的坐标
    solvePnP(markPoint3Ds,markPoint2Ds,cameraMatrix,distCoeffs,rvec,tvec);
    // 验证solvepnp函数的正确性，通过旋转向量和平移向量来得到3D点对应的在图像上的位置
    // vector<Point2f> tempPoint;
    // projectPoints(markPoint3Ds, rvec, tvec, cameraMatrix, distCoeffs, tempPoint);   //还原二维图像
    // cout<<tempPoint.size()<<endl;
    // for(int i = 0; i < tempPoint.size(); i++)
    // {
    //      circle(src, tempPoint[i], 5, Scalar(255));
    // }  

    //罗德里格斯公式，将罗德里格斯向量转换成旋转矩阵
    Rodrigues(rvec,rotateMatrix);
    float theta_z = abs(atan2(rotateMatrix.at<double>(1,0), rotateMatrix.at<double>(0,0))*57.2958);

    float theta_y = abs(atan2(-rotateMatrix.at<double>(2,0), sqrt(rotateMatrix.at<double>(2,0) * rotateMatrix.at<double>(2,0) + rotateMatrix.at<double>(2,2) * rotateMatrix.at<double>(2,2)))*57.2958);

    float theta_x = abs(atan2(rotateMatrix.at<double>(2,1), rotateMatrix.at<double>(2,2))*57.2958);




    hconcat(rotateMatrix,tvec,RT_);
    
    left_up3D    = Calculate(markPoint3Ds[0], RT_);
    right_up3D   = Calculate(markPoint3Ds[1], RT_);
    right_down3D = Calculate(markPoint3Ds[2], RT_);
    left_down3D  = Calculate(markPoint3Ds[3], RT_);
    // cout<<endl;
    // cout<<"=========================="<<endl;
    // cout<<left_up3D<<" "<<right_down3D<<endl;
    // cout<<theta_x<<" "<<theta_y<<" "<<theta_z<<endl;
    // cout<<"=========================="<<endl;
    // cout<<endl;
    marker.pose.position.x = (left_up3D.x + right_down3D.x)/10;
    marker.pose.position.y = -(left_up3D.y + right_down3D.y)/10;
    marker.pose.position.z = -(left_up3D.z + right_down3D.z)/10;
    marker.pose.orientation.x = theta_x/180 * CV_PI;    // 绿轴
    marker.pose.orientation.y = -theta_y/180 * CV_PI;    // 蓝轴
    marker.pose.orientation.z = theta_z/180 * CV_PI;    // 红轴
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 13.5;
    marker.scale.y = 5.5;
    marker.scale.z = 1.0;

    marker.color.r = 1.0f;
    marker.color.g = 1.0f;
    marker.color.b = 1.0f;
    marker.color.a = 0.8;

    msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", src).toImageMsg();

    // cap.open("/home/czs/桌面/water.avi");
    pub1.publish(msg);
    pub2.publish(marker);
    ros::spinOnce();
    looprate.sleep();
    markPoint2Ds.clear();
    markPoint3Ds.clear();
    rotate_Rect_Group.clear();
    rect_Group.clear();
    contours.clear();
    contours_.clear();
    }
}
}